﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Orpheus.Analyzer
{
    public class PeakFinder
    {
        private int minPos;
        private int maxPos;

        public int DetectPeak(double[] data, int minPos, int maxPos)
        {
            int i;
            int peakpos;                // position of peak level
            double peakLevel;            // peak level
            int crosspos1, crosspos2;   // position where the peak 'hump' crosses cutting level
            double cutLevel;             // cutting value
            double groundLevel;          // ground level of the peak
            int gp1, gp2;               // bottom positions of the peak 'hump'

            this.minPos = minPos;
            this.maxPos = maxPos;

            // find absolute peak
            peakpos = minPos;
            peakLevel = data[minPos];
            for (i = minPos + 1; i < maxPos; i++)
            {
                if (data[i] > peakLevel)
                {
                    peakLevel = data[i];
                    peakpos = i;
                }
            }

            // find ground positions.
            gp1 = FindGround(data, peakpos, -1);
            gp2 = FindGround(data, peakpos, 1);

            groundLevel = Math.Max(data[gp1], data[gp2]);

            if (groundLevel < 1e-6) return 0; // ground level too small => detection failed
            if ((peakLevel / groundLevel) < 1.3) return 0; // peak less than 30% of the ground level => no good peak detected

            // calculate 70%-level of the peak
            cutLevel = 0.70f * peakLevel + 0.30f * groundLevel;
            // find mid-level crossings
            crosspos1 = FindCrossingLevel(data, cutLevel, peakpos, -1);
            crosspos2 = FindCrossingLevel(data, cutLevel, peakpos, 1);

            if ((crosspos1 < 0) || (crosspos2 < 0)) return 0; // no crossing, no peak..

            // calculate mass center of the peak surroundings
            return (int)CalculateMassCenter(data, crosspos1, crosspos2);
        }

        private double CalculateMassCenter(double[] data, int firstPos, int lastPos)
        {
            int i;
            double sum;
            double wsum;

            sum = 0;
            wsum = 0;
            for (i = firstPos; i <= lastPos; i++)
            {
                sum += i * data[i];
                wsum += data[i];
            }
            return sum / wsum;
        }

        private int FindCrossingLevel(double[] data,  double level, int peakpos, int direction)
        {
            int pos;

            double peaklevel = data[peakpos];
            Debug.Assert(peaklevel >= level);

            pos = peakpos;
            while ((pos >= minPos) && (pos < maxPos))
            {
                if (data[pos + direction] < level) return pos; // crossing found
                pos += direction;
            }
            return -1;  // not found
        }

        private int FindGround(double[] data, int peakpos, int direction)
        {
            double refvalue;
            int lowpos;
            int pos;
            int climb_count;
            double delta;

            climb_count = 0;
            refvalue = data[peakpos];
            lowpos = peakpos;

            pos = peakpos;

            while ((pos > minPos) && (pos < maxPos))
            {
                int prevpos;

                prevpos = pos;
                pos += direction;

                // calculate derivate
                delta = data[pos] - data[prevpos];
                if (delta <= 0)
                {
                    // going downhill, ok
                    if (climb_count > 0)
                    {
                        climb_count--;  // decrease climb count
                    }

                    // check if new minimum found
                    if (data[pos] < refvalue)
                    {
                        // new minimum found
                        lowpos = pos;
                        refvalue = data[pos];
                    }
                }
                else
                {
                    // going uphill, increase climbing counter
                    climb_count++;
                    if (climb_count > 5) break; // we've been climbing too long => it's next uphill => quit
                }
            }
            return lowpos;
        }
    }
}
